
/***************************************************************************** \
 *                              ColWrapperOpenSG
\*****************************************************************************/

/*! @file
 *
 *  @brief
 *    Wrapper functions, which convert the OpenSG objects into the
 *    corresponding CollDet objects.
 *
 *  Normally these wrapper functions will be used as callback
 *  function, pass to the collision pipeline as a parameter
 *
 *  see makeCollidable
 *
 *  @author Weiyu Yi, wyi@tu-clausthal.de
 *
 */


//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

#include <stdio.h>

#include <OpenSG/OSGConfig.h>
#include <OpenSG/OSGBaseTypes.h>
#include <OpenSG/OSGBaseFunctions.h>
#include <OpenSG/OSGVector.h>
#include <OpenSG/OSGMatrix.h>
#include <OpenSG/OSGGeometry.h>
#include <OpenSG/OSGGeoProperty.h>
#include <OpenSG/OSGQuaternion.h>
#include <OpenSG/OSGGeoFunctions.h>
#include <OpenSG/OSGGeometry.h>
#include <OpenSG/OSGGeoProperty.h>
#include <OpenSG/OSGPrimitiveIterator.h>
#include <OpenSG/OSGFaceIterator.h>
#include <OpenSG/OSGTriangleIterator.h>

#define COL_EXPORT

#include <col_import_export.h>
#include <ColWrapperOpenSG.h>

/// Collision detection namespace
namespace col {

/** callback function which convert the opensg geometry into
 *  corresponding colldet geometry.
 *
 * @param ColGeometry*  pointer to a tobe assigned colldet geometry
 * @param void*   pointer to the opensg geometry object
 *
 * the opensg geometry's pointer has to be changed into void* type for
 * the consistence of the pipeline interface.
 *
 */
bool convertGeometryFromOpenSG( ColGeometry* geo, void* geo_source )
{
	// assert
	
	// clear the old data in geometry
	geo->clearOldData();

    // copy the points
	const osg::Geometry * source = static_cast<const osg::Geometry *> (geo_source);

    const osg::GeoPositions3fPtr pos = osg::GeoPositions3fPtr::dcast( source->getPositions() );
    if ( pos.getCPtr() == NULL )
	{
        fprintf( stderr, "Error, position field empty.\n" );
		return false;
	}

    const osg::MFPnt3f* points = pos->getFieldPtr();
    for ( unsigned int i=0; i < points->size(); ++i )
    {
		Point3 p( (*points)[i][0], (*points)[i][1], (*points)[i][2] );
		geo->editPointsPtr()->push_back( p );
    }

    // copy the primitives
    osg::FaceIterator iter = source->beginFaces();
    osg::FaceIterator iter_end = source->endFaces();

    for ( ; iter != iter_end; ++iter )
    {
		Primitive prim;
		for ( int unsigned i=0; i<iter.getLength(); i++ )
		{
			prim.push_back( iter.getPositionIndex(i) );
		}
		geo->editPrimitivesPtr()->push_back( prim );
    }
	
	// update bbox
	geo->updateBBox();

	return true;
}

/** callback function which convert the opensg matrix into colldet
 *	matrix.
 *
 * @param Matrix4*  pointer to the colldet matrix
 * @param void*  pointer to the opensg matrix
 *
 * see convertGeometryFromOpenSG
 */
bool convertMatrixFromOpenSG( Matrix4* tran, void* tran_source )
{
	const osg::Matrix *source = static_cast<const osg::Matrix *>(tran_source);

	Vector4 vec0( (*source)[0][0],(*source)[0][1], (*source)[0][2], (*source)[0][3] );
	Vector4 vec1( (*source)[1][0],(*source)[1][1], (*source)[1][2], (*source)[1][3] );
	Vector4 vec2( (*source)[2][0],(*source)[2][1], (*source)[2][2], (*source)[2][3] );
	Vector4 vec3( (*source)[3][0],(*source)[3][1], (*source)[3][2], (*source)[3][3] );

	// tran has be allocated but not be initialized
	// so all vertors in matrix must be assigned.
	tran->setCol( 0, vec0 );
	tran->setCol( 1, vec1 );
	tran->setCol( 2, vec2 );
	tran->setCol( 3, vec3 );

	return true;
}

} //namespace col
